package naito_rescue.router;

import rescuecore2.misc.*;
import rescuecore2.misc.geometry.*;
import rescuecore2.worldmodel.*;
import rescuecore2.standard.entities.*;

import naito_rescue.*;
import naito_rescue.agent.*;
import naito_rescue.router.object.*;
import naito_rescue.object.*;
import java.util.*;

public class AStarModule extends AbstractRoutingModule
{
	public AStarModule(NAITOAgent owner, ConnectivityGraph graph){
		super(owner, graph);
	}
    
    public List<EntityID> search(EntityID start, EntityID goal){
    	return search(start, Arrays.asList(goal));
    }
    
    // Straight-Forward search.
    public List<EntityID> search(EntityID start, Collection<EntityID> goals){
    	
    	//----------------
    	// Variables for Logging.
    	//----------------
    	String   context = "AStarModule#search"; //ログ記録用コンテキスト(今どこの処理を実行しているかを残すため)
    	long     startTime, stopTime; //探索処理の開始時間，終了時間
    	int      expandedCount = 0; //セルの拡張数
    	long     average       = 0; 
    	int      prunedCount = 0;

		//----------------
		// Variables for A*.
		//----------------
		PriorityQueue<SearchNode> openList;
		Map<EntityID, SearchNode> closedList;
		SearchNode    currentNode    = null; //今着目しているノード
		boolean       searchComplete = false; //探索処理が完了したかどうか
		Set<EntityID> neighbors      = null; // currentNodeの隣接ノード群を格納する集合
		
		//----------------
		// Process start.
		//----------------
    	//logger.info("Start = " + start + ", goals = " + goals, context);
    	startTime = System.currentTimeMillis(); //探索の開始時間をセットする
    	
    	openList   = new PriorityQueue<SearchNode>();
    	closedList = new HashMap<EntityID, SearchNode>();
    	// スタートノードの登録
    	//   SearchNode(EntityID, 親ノードID, スタートノードからこのノードまでのコスト, このノードからゴールまでのコストの見積り);
        for (EntityID id: goals)
        {
            int heuristicValue = model.getDistance(model.getEntity(id), model.getEntity(start));
            average += heuristicValue;
            openList.add(new SearchNode(start, null, 0, heuristicValue));
        }
        average = average / goals.size();
        
		while (! openList.isEmpty() && ! searchComplete)
		{
			currentNode = openList.remove();
			
			//logger.debug("Current Node = " + currentNode, context+",while");
			if (isGoal(currentNode.getNodeID(), goals))
			{
				//logger.debug("Search complete!", context+",while");
				searchComplete = true;
				break;
			}

			// check if this is closed
			if (closedList.containsKey(currentNode.getNodeID()))
			{
				// check distance of the previous path found to this node
				SearchNode previousNode = closedList.get(currentNode.getNodeID());
				if (previousNode.getPathLength() > currentNode.getPathLength())
				{
					prunedCount++;
					continue;
					//to next whileloop
				}
			}
			// put current node on close list
			expandedCount++;
			closedList.put(currentNode.getNodeID(), currentNode);

			// expand node
			neighbors = graph.getNeighbors(currentNode.getNodeID());
			//logger.debug("currentNode = " + currentNode, context);
			//logger.debug("neighbours = " + neighbors, context);
			for (EntityID id: neighbors)
			{
				if (!closedList.containsKey(id))
				{
					if(checker.isPassableTo(currentNode.getNodeID(), id) /* || isPF */){
						//logger.info("openList.add");
						
						//SearchNodeを順番に辿っていくと，distanceToCurrentNodeで適切なg値が得られる
						int distanceFromParentToCurrentNode = model.getDistance(model.getEntity(id), model.getEntity(currentNode.getNodeID()));
			            int heuristicValue = model.getDistance(model.getEntity(id), model.getEntity(start));
			            
			            openList.add(new SearchNode(id, currentNode, distanceFromParentToCurrentNode, heuristicValue));
	                }else if(!checker.isPassableTo(currentNode.getNodeID(), id)){
	                }
				}
			}
			
			stopTime = System.currentTimeMillis();
			/*
			if((stopTime - startTime) > 500){
				//logger.info("Process time is too much! break loop. ", context);
				isAborted = true;
				break;
			}
			*/
		}
		if (! searchComplete)
		{
			//logger.info("No path found from = " + start + ", to = " + goals, context);
			return null;
		}
		// construct the path
		List<EntityID> path = new LinkedList<EntityID>();

		while (currentNode.getParent() != null)
		{
			path.add(currentNode.getNodeID());
			currentNode = currentNode.getParent();
		}
		Collections.reverse(path);
		
		int totalCost = computePathLength(path);
		
		stopTime = System.currentTimeMillis();
		long processTime = stopTime - startTime;
		//logger.info("Find path: " + ", Total cost = " + totalCost + ", expandedCount = " + expandedCount + ", processTime = " + processTime + "(ms)" + ", pruned count = " + prunedCount, context);
		return path;
    }
    
    // test.
    public List<EntityID> search2(EntityID start, Collection<EntityID> goals){
    	
    	//----------------
    	// Variables for Logging.
    	//----------------
    	String   context = "AStarModule#search2"; //ログ記録用コンテキスト(今どこの処理を実行しているかを残すため)
    	long     startTime, stopTime; //探索処理の開始時間，終了時間
    	int      expandedCount = 0; //セルの拡張数
    	long     average       = 0; 
    	int      prunedCount = 0;

		//----------------
		// Variables for A*.
		//----------------
		PriorityQueue<SearchNode> openList;
		Map<EntityID, SearchNode> closedList;
		SearchNode    currentNode    = null; //今着目しているノード
		boolean       searchComplete = false; //探索処理が完了したかどうか
		Set<EntityID> neighbors      = null; // currentNodeの隣接ノード群を格納する集合
		
		//----------------
		// Process start.
		//----------------
    	//logger.info("Start = " + start + ", goals = " + goals, context);
    	startTime = System.currentTimeMillis(); //探索の開始時間をセットする
    	
    	openList   = new PriorityQueue<SearchNode>();
    	closedList = new HashMap<EntityID, SearchNode>();
    	// スタートノードの登録
    	//   SearchNode(EntityID, 親ノードID, スタートノードからこのノードまでのコスト, このノードからゴールまでのコストの見積り);
        for (EntityID id: goals)
        {
            int heuristicValue = model.getDistance(model.getEntity(id), model.getEntity(start));
            average += heuristicValue;
            openList.add(new SearchNode(start, null, 0, heuristicValue));
        }
        average = average / goals.size();
        
		while (! openList.isEmpty() && ! searchComplete)
		{
			currentNode = openList.remove();
			
			//logger.debug("Current Node = " + currentNode, context+",while");
			if (isGoal(currentNode.getNodeID(), goals))
			{
				//logger.debug("Search complete!", context+",while");
				searchComplete = true;
				break;
			}
			// put current node on close list
			expandedCount++;
			closedList.put(currentNode.getNodeID(), currentNode);

			// expand node
			neighbors = graph.getNeighbors(currentNode.getNodeID());
			for (EntityID id: neighbors)
			{
				if (!closedList.containsKey(id))
				{
					if(checker.isPassableTo(currentNode.getNodeID(), id) /* || isPF */){
						//logger.info("openList.add");
						
						//SearchNodeを順番に辿っていくと，distanceToCurrentNodeで適切なg値が得られる
						int distanceFromParentToCurrentNode = model.getDistance(model.getEntity(id), model.getEntity(currentNode.getNodeID()));
			            int heuristicValue = model.getDistance(model.getEntity(id), model.getEntity(start));
			            
			            openList.add(new SearchNode(id, currentNode, distanceFromParentToCurrentNode, heuristicValue));
	                }else if(!checker.isPassableTo(currentNode.getNodeID(), id)){
	                }
				}
			}
			
			stopTime = System.currentTimeMillis();
			/*
			if((stopTime - startTime) > 500){
				//logger.info("Process time is too much! break loop. ", context);
				isAborted = true;
				break;
			}
			*/
		}
		if (! searchComplete)
		{
			//logger.info("No path found from = " + start + ", to = " + goals, context);
			return null;
		}
		// construct the path
		List<EntityID> path = new LinkedList<EntityID>();

		while (currentNode.getParent() != null)
		{
			path.add(currentNode.getNodeID());
			currentNode = currentNode.getParent();
		}
		Collections.reverse(path);
		
		int totalCost = computePathLength(path);
		
		stopTime = System.currentTimeMillis();
		long processTime = stopTime - startTime;
		//logger.info("Find path: " + ", Total cost = " + totalCost + ", expandedCount = " + expandedCount + ", processTime = " + processTime + "(ms)" + ", pruned count = " + prunedCount + "\n path = " + path, context);
		return path;
    }
}
